﻿import time

import Hobot.GPIO as GPIO
from track_fsm import TrackFsm;


#定义传感器引脚

# 输入引脚
TrackRight = 16
TrackMid = 18
TrackLeft = 22
TrackMinRight = 29
TrackMinLeft = 31

# 输出引脚
LeftWheel_F = 33
LeftWheel_B = 37
RightWheel_F = 32
RightWheel_B = 36

pwm_l = None
pwm_r = None

trackFsm = TrackFsm()
trackFsm.set_state("None")
trackFsm.backup_mode = True



#初始设置GPIO的状态
def setup():
    GPIO.cleanup()
    GPIO.setmode(GPIO.BOARD)
    GPIO.setup(TrackLeft, GPIO.IN,initial=GPIO.HIGH)
    GPIO.setup(TrackMid, GPIO.IN,initial=GPIO.LOW)
    GPIO.setup(TrackRight, GPIO.IN,initial=GPIO.LOW)
    GPIO.setup(TrackMinLeft, GPIO.IN,initial=GPIO.LOW)
    GPIO.setup(TrackMinRight, GPIO.IN,initial=GPIO.LOW)
    # 输出控制
    global pwm_l, pwm_r
    pwm_l = GPIO.PWM(LeftWheel_F,48000)
    pwm_r = GPIO.PWM(RightWheel_F,48000)

    GPIO.setup(LeftWheel_B, GPIO.OUT,initial=GPIO.LOW)
    GPIO.setup(RightWheel_B, GPIO.OUT,initial=GPIO.LOW)

    pwm_l.ChangeDutyCycle(1)#pwm能用
    pwm_r.ChangeDutyCycle(1)#pwm无法变更值
    
    pwm_l.start(0)
    pwm_r.start(0)
    
    
    print("设置完毕")

def set_GPIO_value(lf,lb,rf,rb):
    
    # print("{0} {1} {2} {3}".format(lf,lb,rf,rb))

    
    pwm_l.ChangeDutyCycle(lf)
    pwm_r.ChangeDutyCycle(rf)
    GPIO.output(LeftWheel_B,lb)
    GPIO.output(RightWheel_B,rb)
    

def fsm_init():
    
    # 定义callback
    def none_state_callback():
        # print("执行了none回调")
        set_GPIO_value(0,0,0,0)

    def left_state_callback():
        # print("执行了turnleft回调")
        set_GPIO_value(0,0,65,0)
    
    def right_state_callback():
        # print("执行了turnRight回调")
        set_GPIO_value(65,0,0,0)
    
    def min_left_state_callback():
        # print("执行了minleft回调")
        set_GPIO_value(65,0,75,0)
    
    def min_right_state_callback():
        # print("执行了minright回调")
        set_GPIO_value(75,0,65,0)
    
    def mid_state_callback():
        # print("执行了mid回调")
        set_GPIO_value(70,0,65,0)
    
    
    trackFsm.set_state_drive_callback("None",none_state_callback)
    trackFsm.set_state_drive_callback("TurnLeft",left_state_callback)
    trackFsm.set_state_drive_callback("TurnRight",right_state_callback)
    trackFsm.set_state_drive_callback("TrendLeft",min_left_state_callback)
    trackFsm.set_state_drive_callback("TrendRight",min_right_state_callback)
    trackFsm.set_state_drive_callback("Mid",mid_state_callback)
    
    
# 主循环逻辑--触发器
def loop():
    
    # 记忆变量
    rem_state = None
    
    while True:
        
        # 预获取值
        dec_l = GPIO.input(TrackLeft) == GPIO.LOW
        dec_r = GPIO.input(TrackRight) == GPIO.LOW
        dec_ml = GPIO.input(TrackMinLeft) == GPIO.LOW
        dec_mr = GPIO.input(TrackMinRight) == GPIO.LOW
        dec_m= GPIO.input(TrackMid) == GPIO.LOW
        
        # 循迹传感结果→行驶状态转换
        
       
        # 优化if-else通过字典实现状态查询 元组：状态  状态顺序  左 中 右  微左 微右
        # conditions = {(False,False,False,False,False):"None"}  # 状况1-没有任何循迹反馈
        # #状况2-中路反馈
        # conditions.update({(False,True,False,False,False):"Mid"})
        # #状况3-右转反馈
        # conditions.update({(False,False,True,False,False):"TurnRight"})
        # # 状况4-左转反馈
        # conditions.update({(True,False,False,False,False):"TurnLeft"})
        # # 状况5-微右转反馈
        # conditions.update({(False,False,False,False,True):"TrendRight"})
        # # 状况6-微左转反馈
        # conditions.update({(False,False,False,True,False):"TrendLeft"})
        # 
        # # 额外状态补充1-左和微左同时有信号
        # conditions.update({(True,False,False,True,False):"TurnLeft"})
        # # 额外状态补充2-右和微右同时有信号
        # conditions.update({(False,False,True,False,True):"TurnRight"})
        # # 额外状态补充3-中和微左同时有信号
        # conditions.update({(False,True,False,True,False):"TrendLeft"})
        # # 额外状态补充4-中和微右同时有信号
        # conditions.update({(False,True,False,False,True):"TrendRight"})


        # 另一个传感器调整
        conditions = {(False,False,False,False,False):"None"}  # 状况1-没有任何循迹反馈
        #状况2-中路反馈
        conditions.update({(False,True,False,False,False):"Mid"})
        #状况3-右转反馈
        conditions.update({(True,False,False,False,False):"TurnRight"})
        # 状况4-左转反馈
        conditions.update({(False,False,True,False,False):"TurnLeft"})
        # 状况5-微右转反馈
        conditions.update({(False,False,False,True,False):"TrendRight"})
        # 状况6-微左转反馈
        conditions.update({(False,False,False,False,True):"TrendLeft"})
    
        # 额外状态补充1-左和微左同时有信号
        conditions.update({(False,False,True,False,True):"TurnLeft"})
        # 额外状态补充2-右和微右同时有信号
        conditions.update({(True,False,False,True,False):"TurnRight"})
        # 额外状态补充3-中和微左同时有信号
        conditions.update({(False,True,False,False,True):"TrendLeft"})
        # 额外状态补充4-中和微右同时有信号
        conditions.update({(False,True,False,True,False):"TrendRight"})
        
        
        
        
        # 字典状态映射结果
        for condition,state in conditions.items():
            if condition == (dec_l,dec_m,dec_r,dec_ml,dec_mr):
                trackFsm.set_state(state)
                break
            else: #保底策略
                if  rem_state !=(dec_l,dec_m,dec_r,dec_ml,dec_mr):
                    rem_state = (dec_l,dec_m,dec_r,dec_ml,dec_mr)
                    print("暂时不处理的情况:{0} {1} {2} {3} {4}".format(dec_l,dec_ml,dec_m,dec_mr,dec_r))
                    trackFsm.set_state("None")
                else:
                    # print("重复情况")
                    pass
               

        # time.sleep(1)
        
        # print("R{0} M{1} L{2} MR{3} ML{4}".format(str(GPIO.input(TrackRight)),str(GPIO.input(TrackMid)),str(GPIO.input(TrackLeft)),str(GPIO.input(TrackMinRight)),str(GPIO.input(TrackMinLeft))))
    # 执行电机运转
        trackFsm.apply_motor_drive()
    
    
    
def destroy():
    pwm_l.stop()
    pwm_r.stop()
    GPIO.cleanup()



if __name__ == '__main__':
    setup()
    fsm_init()
    try:
        loop()
    except KeyboardInterrupt:
        destroy()
    finally:
        destroy()
